#include "factor.h"
#include <math.h>

typedef Eigen::Matrix<double, 6, 6> Mat6d;
typedef Eigen::Matrix<double, 6, 1> Vec6d;

Mat6d JRi(Sophus::SE3d& e) {
    Mat6d J;
    J.block(0,0,3,3) = Sophus::SO3d::hat(e.so3().log());
    J.block(0,3,3,3) = Sophus::SO3d::hat(e.translation());
    J.block(3,0,3,3) = Eigen::Matrix3d::Zero(3,3);
    J.block(3,3,3,3) = Sophus::SO3d::hat(e.so3().log());
    J = J * 0.5 + Mat6d::Identity();
    return J;
}

Mat6d JRi(Vec6d e) {
    auto err = Sophus::SE3d::exp(e);
    return JRi(err);
}

node::node(const Sophus::SE3d &pose) {
    _estimate = pose;
}

////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////

void node::oplusImpl(const double *v) {
    Eigen::Map<const Eigen::Matrix<double, 6, 1> > dx(v);
    auto dT = Sophus::SE3d::exp(dx);
    _estimate = dT * _estimate;
}

unary_gps_factor::unary_gps_factor(const Sophus::SE3d &T, Eigen::Matrix<double, 6, 6> &info) {
    _measurement = T.inverse();
    _information = info;
}

void unary_gps_factor::computeError() {
    Sophus::SE3d v1 = (static_cast<node*> (_vertices[0]))->estimate();
    _error = (v1 * _measurement).log();
}

void unary_gps_factor::linearizeOplus() {
    _jacobianOplusXi = JRi(-_error);
}

////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////

binary_gps_factor::binary_gps_factor(const Sophus::SE3d &T,
                                     Eigen::Matrix<double, 6, 6> &info,
                                     double alpha) {
    _measurement = T.inverse();
    _information = info;
    _alpha = alpha;
}

void binary_gps_factor::computeError() {
    Sophus::SE3d v1 = (static_cast<node*> (_vertices[0]))->estimate();
    Sophus::SE3d v2 = (static_cast<node*> (_vertices[1]))->estimate();
    _error = (Sophus::SE3d::exp(_alpha * (v2 * v1.inverse()).log()) * v1 * _measurement).log();
    std::cout << "gps err = \n" << _error << std::endl;
}

void binary_gps_factor::linearizeOplus() {
    Sophus::SE3d v1 = (static_cast<node*> (_vertices[0]))->estimate();
    Sophus::SE3d v2 = (static_cast<node*> (_vertices[1]))->estimate();

    Mat6d JrAd = JRi(_error) * (_measurement.inverse() * v1.inverse()).Adj();
    Vec6d xi21 = (v2 * v1.inverse()).log();
    Mat6d Jra21 = JRi(_alpha * xi21).inverse();
    Mat6d Jr21 = JRi(xi21);
    Mat6d Jl21 = JRi(-xi21);
    _jacobianOplusXi = (JrAd * (Mat6d::Identity() - _alpha * Jra21 * Jr21)).eval();
    _jacobianOplusXj = (_alpha * JrAd * Jra21 * Jl21).eval();
}

////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////

void point::oplusImpl(const double *v) {
    Eigen::Map<const Eigen::Matrix<double, 3, 1> > dx(v);
    _estimate = dx + _estimate;
}

////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////

BAFactor::BAFactor(const Eigen::Vector2d &px, Eigen::Matrix<double, 2, 2> &info, Eigen::Matrix3d& K) {
    Eigen::Vector3d p = K.inverse() * Eigen::Vector3d(px(0), px(1), 1);
    _information = info;
    _measurement = p.normalized();
    a = std::sqrt(_measurement(0) * _measurement(0) + _measurement(1) * _measurement(1));
    b = -_measurement(1) * _measurement(2) / a;
    c = -_measurement(0) * _measurement(2) / a;
}

void BAFactor::computeError() {
    const Sophus::SE3d& v1 = (static_cast<node*> (_vertices[0]))->estimate();
    const Eigen::Vector3d& v2 = (static_cast<point*> (_vertices[1]))->estimate();
    Eigen::Vector3d p = v1 * v2;
    P = p;
    norm = p.norm();
    p = p / norm;
    sin = c * p(0) + b * p(1) + a * p(2);
    _error(0) = std::asin(sin);
    cos = std::sqrt(1 - sin * sin);
    dot = _measurement.dot(p);
    cos2 = dot / cos;
    _error(1) = std::acos(cos2);
    std::cout << "ba error = \n" << _error << std::endl;
}

void BAFactor::linearizeOplus() {
    const Sophus::SE3d& v1 = (static_cast<node*> (_vertices[0]))->estimate();
    double abc_cos = (a + b + c) / cos;
    Eigen::Vector3d p = P / norm;
    double c2 = std::sqrt(1 - cos2 * cos2) * cos;
    double c_ = c2 , b2 = c2 * cos * cos * 2;
    double a2 = p(2) / c2 - (a * dot) / b2;
    c2 = p(0) / c2 - (c * dot) / b2;
    b2 = p(1) / c_ - (b * dot) / b2;
    double abc2_cos = a2 + b2 + c2;
    Eigen::Matrix<double, 2, 3> Jl;
    double r3 = norm * norm * norm;
    Jl(0, 0) = c / cos / norm + P(0) / r3 * abc_cos;
    Jl(0, 1) = b / cos / norm + P(1) / r3 * abc_cos;
    Jl(0, 2) = a / cos / norm + P(2) / r3 * abc_cos;
    Jl(1, 0) = c2 / norm + P(0) / r3 * abc2_cos;
    Jl(1, 1) = b2 / norm + P(1) / r3 * abc2_cos;
    Jl(1, 2) = a2 / norm + P(2) / r3 * abc2_cos;
    Eigen::Matrix<double, 3, 6> Jp;
    Jp.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
    Jp.block<3, 3>(0, 3) = Sophus::SO3d::hat(P);
    _jacobianOplusXi = Jl * Jp;
    _jacobianOplusXj = Jl * v1.so3().matrix();
}
